/*=+--+=#=+--     Unmanned Aerial System Management Software      --+=#=+--+=#*\
|          Copyright (C) 2011 Regents of the University of Colorado.           |
|                             All Rights Reserved.                             |

     This program is free software: you can redistribute it and/or modify
     it under the terms of the GNU General Public License version 2 as
     published by the Free Software Foundation.

     This program is distributed in the hope that it will be useful,
     but WITHOUT ANY WARRANTY; without even the implied warranty of
     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     GNU General Public License for more details.

     You should have received a copy of the GNU General Public License
     along with this program.  If not, see <http://www.gnu.org/licenses/>.

            Jack Elston                       Cory Dixon                        
|           elstonj@colorado.edu              dixonc@colorado.edu              |
|                                                                              |
\*=+--+=#=+--                 --+=#=+--+=#=+--                    --+=#=+--+=#*/

/*!
  \file template.cxx
	\brief class implemenation for template

                    \author Jack Elston
             $Date: 2005/02/12 00:20:45 $
*/

#include <unistd.h>
#include <signal.h>
#include <iostream>
#include <stdlib.h>
#include <stdio.h>
#include <errno.h>
#include <fcntl.h>

#include "testing.h"

/*<-------Defines--------->*/
/*<------End Defines------>*/

/*<---Global Variables---->*/
extern bool verbose;
/*<-End Global Variables-->*/

Testing::Testing(String name) : ThreadedPipe(name)
{
	pending_input = false;
	input_length = 0;
}

void Testing::update() 
{
	int val, maxFD;
	fd_set readFDs, writeFDs;                   // fd sets
	struct timeval timeout;

	FD_ZERO(&readFDs);
	FD_ZERO(&writeFDs);

	FD_SET(fifo_FDs[INBOARD_RX],&readFDs);
	FD_SET(fifo_FDs[INBOARD_TX],&writeFDs);
	maxFD = fifo_FDs[INBOARD_RX];
	maxFD = (fifo_FDs[INBOARD_TX] > maxFD ? fifo_FDs[INBOARD_TX] : maxFD);

	timeout.tv_sec = TIME_OUT_SEC; timeout.tv_usec = TIME_OUT_USEC;

	if( (val = select(maxFD + 1, &readFDs, &writeFDs,NULL,&timeout)) <= 0) {
		if( val != 0 ) {
			if (errno != EINTR) {
				perror("Testing::update() select");
				execRate = -1;
			} else
				cout << getName() << "::update() - select interrupted" << endl;
		}
	}
	else {
		if(FD_ISSET(fifo_FDs[INBOARD_RX], &readFDs)) {
			int n = 0;
			n=readFifo(INBOARD_RX);
			if(n>0) {
				uint16_t length = CMD_SIZE(fifo_msg.data);
				if( length != fifo_msg.size) {
					cout << getName() << "::update() - error, length of command != length of buffer" << endl
						<< "\t CMD_SIZE=" << (int)length << " fifo_msg.size=" << (int)fifo_msg.size << endl;

					if( length < 1000 && length <= fifo_msg.size )
						if( C_calcCheckSum((uint8_t *)buf) == 0 ) 
							cout << "\tCMD: is valid" << endl;
						else
							cout << "\tCMD: is NOT valid" << endl;

					return;
				}

				Command cmd((uint8_t *)fifo_msg.data);

				if( !cmd.isValid() )
					cout << getName() << "::update() - error, invalid command" << endl << cmd ;
				else {
					Client * a_client;
					a_client = shmPtr->clients.getClient(fifo_msg.src);

					uint8_t * dataPtr = CMD_DATAPTR(fifo_msg.data);

					switch(cmd.cmd() ) {
						case CMD_CUSTODY: {
								Custody_t * custody  = (Custody_t*)dataPtr;
								Client * me = shmPtr->clients.getClient(0);
								cout << endl << "Custody response from " << a_client->name << " with " << inet_ntoa(custody->parent) <<   endl << endl;
								cout << "I am " << inet_ntoa(shmPtr->clients.getAddr(me)) <<  endl;
								if(custody->parent.s_addr == shmPtr->clients.getAddr(me).s_addr) {
									cout << "I have custody" << endl;
									children.add(a_client);
								}
								else {
									if(children.getClient(a_client->name) != NULL) {
										children.remove(a_client);
									}
								}
							} break;
						case CMD_GPS_TELEM: {
								/*GPSStruct_t * telemetry  = (GPSStruct_t*)dataPtr;
									Client * tracked_client = tracked_vehicles.getClient(a_client->name);
									if(tracked_client != NULL) {
									tracked_client->telemetry.pos.lat=telemetry->pos.lat;
									tracked_client->telemetry.pos.lon=telemetry->pos.lon;
									tracked_client->telemetry.track.tru=telemetry->track.tru;
									tracked_client->telemetry.airdata.trueAirSpeed=(int16_t)(telemetry->sog.value);
									}

									bool got_info = true;
									for(int i=0; i< tracked_vehicles.getNumClients(); i ++)
									if(((tracked_vehicles.getClient(i))->telemetry.pos.lat) == 0.0)
									got_info = false;
									if((tracked_vehicles.getNumClients() > 0) && got_info) { trackTargets(); } */
							} break;
						default:
							break;
					}
				}
			}
		}
	}
	getUserInput();
}

void Testing::getUserInput() {
	if(!pending_input) {
		cout << "Enter Command Type (custody,coordinate) and ID: " << flush;
		pending_input=true;
	}
	else {
		char c;
		c = getchar_unlocked();
		if(c != EOF) {
			if(c == '\n') {input[input_length] = 0; parseUserInput();}
			else {input[input_length] = c; input_length ++;}
		}
	}
}

void Testing::parseUserInput() {
	char command [80];
	char option [80];
	Client * a_client;
	if(sscanf(input,"%s %s", command, option)) {
		if(strcmp("custody",command)==0) {
			a_client = shmPtr->clients.getClient(option);
			if(a_client == NULL) {
				cout << "improper ID or not a client" << endl;
			}
			else reqCustody(a_client,true);
		}
		else if(strcmp("coordinate",command)==0) {
			a_client = shmPtr->clients.getClient(option);
			if(a_client == NULL) {
				cout << "improper ID or not a client" << endl;
			}
			else reqCoordinate(a_client,true);
		}
	}
	else cout << "Improper Format" << endl;
	input_length=0;
	pending_input=false;
}

void Testing::reqTelem(Client * a_client, bool toggle) {
	Client * me = shmPtr->clients.getClient(0);
	CMD_CMD(fifo_msg.data) = CMD_STREAM_REQ;
	CMD_NUM(fifo_msg.data) = sizeof(StreamReq_t);
	StreamReq_t * request = (StreamReq_t*)CMD_DATAPTR(fifo_msg.data);

	request->id = CMD_TELEM;
	request->client.s_addr = shmPtr->clients.getAddr(me).s_addr;
	request->port = 1000;
	request->send_data = toggle;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(INBOARD_TX);
}

void Testing::reqCustody(Client * a_client, bool toggle) {

	Client * me = shmPtr->clients.getClient(0);
	CMD_CMD(fifo_msg.data) = CMD_CUSTODY;

	CMD_NUM(fifo_msg.data) = sizeof(Custody_t);
	Custody_t * request = (Custody_t*)CMD_DATAPTR(fifo_msg.data);

	request->parent.s_addr = shmPtr->clients.getAddr(me).s_addr;
	request->requesting=toggle;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(INBOARD_TX);
	cout << "SENT REQUEST TO " << a_client->name << endl;
}

void Testing::reqCoordinate(Client * a_client, bool toggle) {

	CMD_CMD(fifo_msg.data) = CMD_COORDINATE;
	CMD_NUM(fifo_msg.data) = sizeof(Coordinate_t);
	Coordinate_t * request = (Coordinate_t*)CMD_DATAPTR(fifo_msg.data);

	request->partner.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
	request->requesting = toggle;
	CMD_CSUM(fifo_msg.data) = 0;
	CMD_CSUM(fifo_msg.data) = C_calcCheckSum((uint8_t*)fifo_msg.data);

	fifo_msg.dest.s_addr = shmPtr->clients.getAddr(a_client).s_addr;
	fifo_msg.size = CMD_SIZE(fifo_msg.data);
	writeFifo(INBOARD_TX);
}
